#include "dev_motor.h"

/************************************************

             麦克纳姆轮安装方向

        A轮  \\   ------    //  B轮
			  \\  ------   //
				  ------
				  ------				
				  ------
			 //   ------  \\
	    D轮 //    ------   \\   C轮
				
			//四驱底盘及四轮麦克纳姆轮底盘					   
*************************************************
*/


typedef struct
{
	uint8_t car_active;
	uint8_t car_type;
	uint8_t car_speed;
	uint8_t car_dir;
}motor_private_t;


motor_private_t dev_motor;



/****************************************************************************私有函数************************************************************************************/
/**
* @brief  电机控制任务
	* @parm   None
	* @retval None
	*/
static void Dev_Motor_Task(void *pvParameter)
{
	while(1)
	{
		if(dev_motor.car_active)
		{
			if(dev_motor.car_type == 0) //0麦克纳木轮子   1 普通轮子  2 履带
			{

			}
			else if(dev_motor.car_type == 1) //0麦克纳木轮子   1 普通轮子  2 履带
			{

			}
			else if(dev_motor.car_type == 2) //0麦克纳木轮子   1 普通轮子  2 履带
			{

			}	
		}
		else
		{
			Drv_wirte_motor_duty(MOTOR_A,0);
			Drv_wirte_motor_duty(MOTOR_B,0);
			Drv_wirte_motor_duty(MOTOR_C,0);
			Drv_wirte_motor_duty(MOTOR_D,0);
		}
		vTaskDelay(10);
	}
}


/****************************************************************************接口函数************************************************************************************/
/**
* @brief  电机控制使能
	* @parm   None
	* @retval None
	*/
void Dev_Motor_Active(uint8_t active)
{
	if(active)
	{
		dev_motor.car_active = 1;
	}
	else
	{
		dev_motor.car_active = 0;
	}
}
/**
	* @brief  电机速度全部设置
	* @parm   None
	* @retval None
	*/
void Dev_Motor_Speed_All_Set(uint8_t speed)
{
	if(speed > 100) {speed = 100;}
	
	dev_motor.car_speed = speed;

	if(dev_motor.car_active)
	{
		if(dev_motor.car_type == 0) //0麦克纳木轮子   1 普通轮子  2 履带
		{
			Drv_wirte_motor_duty(MOTOR_A,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_B,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_C,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_D,dev_motor.car_speed);
		}
		else if(dev_motor.car_type == 1) //0麦克纳木轮子   1 普通轮子  2 履带
		{
			Drv_wirte_motor_duty(MOTOR_A,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_B,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_C,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_D,dev_motor.car_speed);
		}
		else if(dev_motor.car_type == 2) //0麦克纳木轮子   1 普通轮子  2 履带
		{
			Drv_wirte_motor_duty(MOTOR_A,dev_motor.car_speed);
			Drv_wirte_motor_duty(MOTOR_B,dev_motor.car_speed);
		}	
	}
	else
	{
		Drv_wirte_motor_duty(MOTOR_A,0);
		Drv_wirte_motor_duty(MOTOR_B,0);
		Drv_wirte_motor_duty(MOTOR_C,0);
		Drv_wirte_motor_duty(MOTOR_D,0);
		
		Dev_Car_Stop();
	}
}
/**
* @brief 电机速度单独设置 
* @parm   speed_A speed_B speed_C speed_D 速度范围 [-100,100] 负的为反转 正的为正转
	* @retval None 
	*/
void Dev_Motor_Speed_Alone_Set(int8_t speed_A,int8_t spee_B,int8_t speed_C,int8_t speed_D)
{
	if(dev_motor.car_active)
	{
		if(speed_A>0)
		{
			MOTOR_A_DIR(GO);
			Drv_wirte_motor_duty(MOTOR_A,speed_A);
		}
		else if(speed_A<0)
		{
			MOTOR_A_DIR(BACK);
			Drv_wirte_motor_duty(MOTOR_A,-speed_A);
		}
		else 
		{
			MOTOR_A_DIR(STOP);
			Drv_wirte_motor_duty(MOTOR_A,0);
		}
		
		if(spee_B > 0)
		{
			MOTOR_B_DIR(GO);
			Drv_wirte_motor_duty(MOTOR_B,spee_B);
		}
		else if(spee_B < 0)
		{
			MOTOR_B_DIR(BACK);
			Drv_wirte_motor_duty(MOTOR_B,-spee_B);
		}
		else
		{
			MOTOR_B_DIR(STOP);
			Drv_wirte_motor_duty(MOTOR_B,0);
		}
		
		if(speed_C > 0)
		{
			MOTOR_C_DIR(GO);
			Drv_wirte_motor_duty(MOTOR_C,speed_C);
		}
		else if(speed_C < 0)
		{
			MOTOR_C_DIR(BACK);
			Drv_wirte_motor_duty(MOTOR_C,speed_C);
		}
		else
		{
			MOTOR_C_DIR(STOP);
			Drv_wirte_motor_duty(MOTOR_C,0);
		}
		
		if(speed_D > 0)
		{
			MOTOR_D_DIR(GO);
			Drv_wirte_motor_duty(MOTOR_D,speed_D);
		}
		else if(speed_D < 0)
		{
			MOTOR_D_DIR(BACK);
			Drv_wirte_motor_duty(MOTOR_D,speed_D);
		}
		else
		{
			MOTOR_D_DIR(STOP);
			Drv_wirte_motor_duty(MOTOR_D,0);
		}
	}
	else
	{
		Drv_wirte_motor_duty(MOTOR_A,0);
		Drv_wirte_motor_duty(MOTOR_B,0);
		Drv_wirte_motor_duty(MOTOR_C,0);
		Drv_wirte_motor_duty(MOTOR_D,0);
		
		Dev_Car_Stop();
	}
}

/**
* @brief  前进
	* @parm   None
	* @retval None
	*/
void Dev_Car_Go(void)
{
	//左前电机 前    //右前电机 前
	MOTOR_A_DIR(GO);       MOTOR_B_DIR(GO);
	//左后电机 前   //右后电机 前
	MOTOR_D_DIR(GO);       MOTOR_C_DIR(GO);
}
/**5
* @brief  后退
	* @parm   None
	* @retval None
	*/
void Dev_Car_Back(void)
{
		//左前电机 后    //右前电机 后
	MOTOR_A_DIR(BACK);       MOTOR_B_DIR(BACK);
		//左后电机 后   //右后电机 后
	MOTOR_D_DIR(BACK);       MOTOR_C_DIR(BACK);
}
/**
* @brief  向左
	* @parm   None
	* @retval None
	*/
void Dev_Car_Left(void)
{
	//左前电机 后    //右前电机 前
	MOTOR_A_DIR(BACK);       MOTOR_B_DIR(GO);
	//左后电机 前   //右后电机 后
	MOTOR_D_DIR(GO);       MOTOR_C_DIR(BACK);
}
/**
* @brief  向左前45度
	* @parm   None
	* @retval None
	*/
void Dev_Car_LeftQ45(void)
{
	//左前电机 停    //右前电机 前
	MOTOR_A_DIR(STOP);   MOTOR_B_DIR(GO);
	//左后电机 前   //右后电机 停
	MOTOR_D_DIR(GO);       MOTOR_C_DIR(STOP);
}
/**
* @brief  向左后45度
	* @parm   None
	* @retval None
	*/
void Dev_Car_LeftH45(void)
{
	//左前电机 后    //右前电机 停
	MOTOR_A_DIR(BACK);       MOTOR_B_DIR(STOP);
	//左后电机 停   //右后电机 后
	MOTOR_D_DIR(STOP);       MOTOR_C_DIR(BACK);
}
/**
* @brief  向左转圈
	* @parm   None
	* @retval None
	*/
void Dev_Car_Turn_Left(void)
{
	//左前电机 后    //右前电机 前
	MOTOR_A_DIR(BACK);       MOTOR_B_DIR(GO);
	//左后电机 后   //右后电机 前
	MOTOR_D_DIR(BACK);       MOTOR_C_DIR(GO);
}
/**
* @brief  向右
	* @parm   None
	* @retval None
	*/
void Dev_Car_Right(void)
{
	//左前电机 前    //右前电机 后
	MOTOR_A_DIR(GO);       MOTOR_B_DIR(BACK);
	//左后电机 后   //右后电机 前
	MOTOR_D_DIR(BACK);       MOTOR_C_DIR(GO);
}
/**
* @brief  向右前45度
	* @parm   None
	* @retval None
	*/
void Dev_Car_RightQ45(void)
{
	//左前电机 前    //右前电机 停
	MOTOR_A_DIR(GO);       MOTOR_B_DIR(STOP);
	//左后电机 停   //右后电机 前
	MOTOR_D_DIR(STOP);       MOTOR_C_DIR(GO);
}
/**
* @brief  向右后45度
	* @parm   None
	* @retval None
	*/
void Dev_Car_RightH45(void)
{
	//左前电机 停    //右前电机 后
	MOTOR_A_DIR(STOP);       MOTOR_B_DIR(BACK);
	//左后电机 后   //右后电机 停
	MOTOR_D_DIR(BACK);       MOTOR_C_DIR(STOP);
}
/**
* @brief  向右转圈
	* @parm   None
	* @retval None
	*/
void Dev_Car_Turn_Right(void)
{
	//左前电机 前    //右前电机 后
	MOTOR_A_DIR(GO);       MOTOR_B_DIR(BACK);
	//左后电机 前   //右后电机 后
	MOTOR_D_DIR(GO);       MOTOR_C_DIR(BACK);
}
/**
* @brief  停车
	* @parm   None
	* @retval None
	*/
void Dev_Car_Stop(void)
{
	//左前电机 停    //右前电机 停
	MOTOR_A_DIR(STOP);       MOTOR_B_DIR(STOP);
	//左后电机 停   //右后电机 停
	MOTOR_D_DIR(STOP);       MOTOR_C_DIR(STOP);
}

/**
* @brief 电机控制组件
	* @parm   None
	* @retval None
	*/
void Dev_Motor_Init()
{
	Dev_Car_Stop();
	
	xTaskCreate(Dev_Motor_Task, "Dev_Motor_Task", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+3, NULL);
}
